"""
地球物理变换，包含经纬度距离双向换算。

date: 2025/7/8
author: SiHeng Tang
file: geo_math_trans.py
copyright(c) DFSA Eastern Software
此程序不能提供任何担保 WITHOUT WARRANTY OF ANY KIND
"""
import math

from geopy.distance import geodesic, EARTH_RADIUS

from mj_errors import MJRuntimeError

EARTH_RADIUS_METERS = EARTH_RADIUS * 1000


# 这东西实现就看着乱，使用直接看注释就行
# 椭球距离和 simple 距离 160 米差 0.2 米，差不多百分之一精度
# 注意弧度制和角度制， math 库里的三角函数使用弧度制
class GeoVector:
    """
    地理向量是一个多用途类，用于进行经纬度和二维变换，为无人机短距离飞行导航优化。
    地理向量对于输入处理有两种类别，一种是地理位置，一种是增量向量，前者被用于保存一个点，后者用于参与运算和保存中间变量。
    两者的初始化方式不同::

        position = GeoVector(lat=39.9042, lon=116.4074)
        d_vector = GeoVector(dx=50, dy=50)

    地理位置会把输入解释为角度制经纬度，增量向量将输入解释为米。
    默认的坐标和经纬度对应关系为：y 正方向和维度北方向，x 正方向和经度东方向。

    增量矢量运算与普通二维矢量运算结果完全相同。
    对于将地理位置参与计算有多种方式，地理位置之间只有作差计算，结果为一个从起点指向终点的增量向量，
    这里增量向量的模长由距离换算得到，小于 100 公里的距离上使用 haversine 公式，大于 100 公里的距离上使用 vincenty 公式，
    方向与位置在较短距离上再正东和正北两个方向上正交分解，较远距离计算也按此规则进行，但是没有实际物理意义。
    增量向量用于对一个目标点进行变换或相互计算得到新结果。一般而言对于地球尺度，增量向量采用的米数量级都是小量，
    对于将一个位置加上增量向量得到新位置的计算是近似计算。
    使用示例::

        # 定义位置点
        pos0, pos1 = GeoVector(lat=39.9042, lon=116.4074), GeoVector(lat=31.2207, lon=121.4579)
        # 定义增量矢量
        d_vector = GeoVector(dx=50, dy=50)

        # 位置运算
        pos0.distance(pos1) # 返回这两个位置之间的绝对值
        pos0.diff(pos1) # 返回这两个位置之间的增量向量
        pos0.add_difference(d_vector) # 将一个位置加上一个向量

        # 增量矢量运算通数学矢量
        dv0, dv1 = ...
        dv0 + dv1, dv0 - dv1, dv0.dot(dv1), dv0.cross(dv1)
    """
    POSITION_TYPE = 0
    VECTOR_TYPE = 1

    def __init__(self, **kwargs):
        if ("lat" in kwargs.keys()) and ("lon" in kwargs.keys()):
            self._type = self.POSITION_TYPE
            self._lat = float(kwargs["lat"])
            self._lon = float(kwargs["lon"])
        elif ("dx" in kwargs.keys()) and ("dy" in kwargs.keys()):
            self._type = self.VECTOR_TYPE
            self._dx = float(kwargs["dx"])
            self._dy = float(kwargs["dy"])
        else:
            raise MJRuntimeError(MJRuntimeError.INTERNAL_ERROR, "invalid GeoVector input")

    @property
    def lat(self):
        if self._type == self.POSITION_TYPE:
            return self._lat
        else:
            raise AttributeError("Vector type does not have lat attribute")

    @property
    def lon(self):
        if self._type == self.POSITION_TYPE:
            return self._lon
        else:
            raise AttributeError("Vector type does not have lon attribute")

    @property
    def dx(self):
        if self._type == self.VECTOR_TYPE:
            return self._dx
        else:
            raise AttributeError("Position type does not have dx attribute")

    @property
    def dy(self):
        if self._type == self.VECTOR_TYPE:
            return self._dy
        else:
            raise AttributeError("Position type does not have dy attribute")

    @staticmethod
    def _simple_distance(lat1, lon1, lat2, lon2):
        lat1, lon1, lat2, lon2 = map(math.radians, [lat1, lon1, lat2, lon2])
        delta_lat, delta_lon = lat2 - lat1, lon2 - lon1
        delta_x, delta_y = delta_lon * EARTH_RADIUS_METERS * math.cos(lat1), delta_lat * EARTH_RADIUS_METERS
        return math.sqrt(delta_x ** 2 + delta_y ** 2)

    @staticmethod
    def _vincenty_distance(lat1, lon1, lat2, lon2):
        return geodesic((lat1, lon1), (lat2, lon2)).km * 1000.

    @staticmethod
    def _verify_type(self, other, l_type, r_type):
        if self._type != l_type or other._type != r_type:
            raise TypeError("Two GeoVector can not be operated")

    @staticmethod
    def get_gv_from_hdg(hdg: float):
        hdg_rad = math.radians(hdg)
        return GeoVector(dx=math.sin(hdg_rad), dy=math.cos(hdg_rad))

    @staticmethod
    def get_hdg_from_gv(gv) -> float:
        if gv._type != gv.VECTOR_TYPE:
            raise TypeError("Can not trans position to HDG")
        unit_vector = gv.get_unit_vector()
        hdg_pi = math.acos(unit_vector.dy)
        if unit_vector.dx < 0:
            hdg_pi = math.pi * 2 - hdg_pi
        hdg = hdg_pi / math.pi * 180.
        return hdg

    def distance(self, other, force_vincenty=False):
        self._verify_type(self, other, self.POSITION_TYPE, self.POSITION_TYPE)

        if (self._simple_distance(self._lat, self._lon, other._lat, other._lon) < 1e5) and (not force_vincenty):
            return self._simple_distance(self._lat, self._lon, other._lat, other._lon)
        else:
            return self._vincenty_distance(self._lat, self._lon, other._lat, other._lon)

    def diff(self, other):
        self._verify_type(self, other, self.POSITION_TYPE, self.POSITION_TYPE)

        distance = self.distance(other)
        lat_diff, lon_diff = other._lat - self._lat, other._lon - self._lon

        # 距离过近认为是同一个点
        if math.sqrt((lon_diff * math.cos(math.radians(self.lat))) ** 2 + lat_diff ** 2) < 1e-5:
            return GeoVector(dx=0, dy=0)

        dx = (distance * (lon_diff * math.cos(math.radians(self.lat))) /
              math.sqrt((lon_diff * math.cos(math.radians(self.lat))) ** 2 + lat_diff ** 2))
        dy = (distance * lat_diff /
              math.sqrt((lon_diff * math.cos(math.radians(self.lat))) ** 2 + lat_diff ** 2))
        return GeoVector(dx=dx, dy=dy)

    def add_difference(self, vector):
        self._verify_type(self, vector, self.POSITION_TYPE, self.VECTOR_TYPE)

        lat_diff = math.degrees(vector.dy / EARTH_RADIUS_METERS)
        lon_diff = math.degrees(vector.dx / (EARTH_RADIUS_METERS * math.cos(math.radians(self._lat))))
        return GeoVector(lat=self._lat + lat_diff, lon=self._lon + lon_diff)

    def __add__(self, other):
        self._verify_type(self, other, self.VECTOR_TYPE, self.VECTOR_TYPE)

        return GeoVector(dx=self._dx + other._dx, dy=self._dy + other._dy)

    def __sub__(self, other):
        self._verify_type(self, other, self.VECTOR_TYPE, self.VECTOR_TYPE)

        return GeoVector(dx=self._dx - other._dx, dy=self._dy - other._dy)

    def __mul__(self, other):
        if self._type != self.VECTOR_TYPE and type(other) != float:
            raise TypeError("operation require not reachable")

        return GeoVector(dx=self.dx * other, dy=self.dy * other)

    def dot(self, other):
        self._verify_type(self, other, self.VECTOR_TYPE, self.VECTOR_TYPE)
        return self._dx * other._dx + self._dy * other._dy

    def cross(self, other):
        self._verify_type(self, other, self.VECTOR_TYPE, self.VECTOR_TYPE)

        return self._dx * other._dy - self._dy * other._dx

    def get_unit_vector(self):
        if self._type != self.VECTOR_TYPE:
            raise TypeError("position vector has no unit vector")

        # 零向量没有单位向量
        if self.length() < 1e-5:
            raise TypeError("zero vector has no unit vector")

        return GeoVector(dx=self.dx / self.length(), dy=self.dy / self.length())

    def length(self):
        if self._type != self.VECTOR_TYPE:
            raise TypeError("position vector has mo length")
        return math.sqrt(self.dx ** 2 + self.dy ** 2)

    def __str__(self):
        if self._type == self.POSITION_TYPE:
            return f"GeoVector Position(lat={self._lat}, lon={self._lon})"
        else:
            return f"GeoVector Vector(dx={self._dx}, dy={self._dy})"


if __name__ == '__main__':
    # 测试地理位置
    pos1 = GeoVector(lat=39.9042, lon=116.4074)
    pos2 = GeoVector(lat=39.9050, lon=116.4090)
    d_vector1 = GeoVector(dx=50, dy=50)
    d_vector2 = GeoVector(dx=30, dy=20)

    print("Position 1:", pos1)
    print("Position 2:", pos2)
    print("Vector 1:", d_vector1)
    print("Vector 2:", d_vector2)
    print("\n")

    # 测试地理位置之间的距离
    distance = pos1.distance(pos2)
    distance2 = pos1.distance(pos2, force_vincenty=True)
    print("Distance between positions (km):", distance)
    print("Distance between positions v (km):", distance2)
    print("\n")

    # 测试地理位置之间的差向量
    diff_vector = pos1.diff(pos2)
    print("Difference vector from pos1 to pos2:", diff_vector)
    print("\n")

    # 测试增量向量的加法和减法
    sum_vector = d_vector1 + d_vector2
    print("Sum of vectors:", sum_vector)
    sub_vector = d_vector1 - d_vector2
    print("Subtraction of vectors:", sub_vector)
    print("\n")

    # 测试增量向量的点积和叉积
    dot_product = d_vector1.dot(d_vector2)
    print("Dot product of vectors:", dot_product)
    cross_product = d_vector1.cross(d_vector2)
    print("Cross product of vectors:", cross_product)
    print("\n")

    # 测试位置加上增量向量
    new_pos = pos1.add_difference(d_vector1)
    print("New position after adding vector:", new_pos)
    print("Delta position:", pos1.diff(new_pos))

    unit_vector = d_vector1.get_unit_vector()
    new_pos = pos1.add_difference(unit_vector * 70.)
    print("unit trans ", unit_vector)
    print("Add with unit mul vector ", new_pos)

    unit_gv_hdg = GeoVector.get_gv_from_hdg(351)
    print("Get unit vector from hdg: ", unit_gv_hdg)
    hdg = GeoVector.get_hdg_from_gv(unit_gv_hdg)
    print("Get hdg from vector: ", hdg)
